import random
import math
import pygame
import sys
from pprint import pprint
from math import pi
from math import sqrt
from collections import deque
from astree import Point
from astree import ASTree
from readLevel import ReadLevel
import time

first = 0
level = None
tempWalls = []
com_enemy_shot = {} # enemies who will be shot by teammate
com_ammo_nav = {} # ammo points which will be taken by teammate
com_ammo_av = {} # ammo points currently available to teammates
com_enemy_vis = [] # enemies visible (and if theyre near a CP/their location) (CPidNearLast, lastCoords, camping?, roundLastSeen)
com_agents = [] # (ammo, controlpointID, camping (0 = on way, 1 = camping, 2 = leave spot, 3 = no camping), camperspot?)
#com_agents[observation['id'] - 1]
com_controlPoints = [0, 0, 0] #0 = neutral, 1 = ours, 2 is ours + defending (=sitting on it)
# 3 is ours + defending with ammo, 4 = 3 + helper, 5 = 3+helper w ammo
#-1 is taken by opponent, -2 is taken and defended on spot, -3 + ammo
com_controlAgentNearP = [0, 0, 0] # whether one of our agent could see this point last round
com_controlPScore = [300, 300, 300] # heuristic scores for the control points
# TODO: make a reasonable heuristic score ;). Higher score -> more reason to go there
# current idea: -decrease if it's ours and agent is near? decrease max 100, if close + point is ours
#               -increase if it's neutral/theirs for longer time? negatory, just keep it.
#                -Increase if enemy spotted (50 per uniq enemy/turn?)
#
MAX_SPEED = 40
MAX_SHOT_DISTANCE  = 60
MAX_SIGHT = 100
AGENT_SIZE = 5 # radius
CONTROLPOINT_SIZE = 10 #radius

WEIGHT_AM_BASE = 310
WEIGHT_AM_AMMO = 5
WEIGHT_AM_MOVES = 1
WEIGHT_BESTCP_PREV = 5
WEIGHT_ENEMY_SBonus = 30

round = 0

class AgentBrain():
    def __init__(self):
        self.goalID = 0 # it will get a new goal later on anyway
        self.goalScore = 100
        self.expQueue = []
        self.path = []
        self.lastHeuristicDecrease = 0

    def action(self, observation):
        global first, level, com_enemy_shot, com_ammo_nav, com_controlPScore
        global round, first, MAX_goto_flag, directions, com_agents, com_controlPoints
        global com_enemy_nav, com_controlAgentNearP, com_enemy_vis, tempWalls

        self.team = observation['team']
        if first == 0:
            #TIM
            #TODO: CLONE FULL AS TREE FOR CLEAN SHARED MAP AT EACH ROUND
            #NEXT: ADD OTHER AGENTS AS POINTS AND WALLS (UPDATE SIGHT) IN MAP
            first = observation['id']
        #    level = ReadLevel('levels\\clover2.py')
            for i in range(8):
                com_agents.append([0,0,0,0])
                com_enemy_vis.append([-1,(-1,-1),-1,0])


        level = ReadLevel('levels\\clover2.py')
        tempWalls = []

        if first == observation['id']:
            round += 1
            com_enemy_shot = {}
            com_ammo_nav = {}
            com_ammo_av = {}


            #Remove temp nodes
            #level.tree.cleanMap()

            for i in range(3): # process control point observations
                if com_controlPoints[i] <= 0 and self.team == observation['controlpoints'][i]['team']:
                    com_controlPoints[i] = 1 # setting it to 2 or 3 happens when that agent checks it
                    #print '%s we took control %i' % (self.team, i)
                    if com_controlPScore[i] > 200:
                        com_controlPScore[i] = 200
                elif com_controlPoints[i] >= 0 and self.team != observation['controlpoints'][i]['team'] \
                        and 'Neutral' != observation['controlpoints'][i]['team']:
                    com_controlPoints[i] = -1 # opponent took the point last turn
                    #print '%s we lost control %i' % (self.team, i)
                    # if we saw an enemy at this point last time, we assume it's still their (why -> so we only
                    #  go to this point when we got ammo)
                    if com_controlPScore[i] < 300:
                        com_controlPScore[i] = 300
                elif com_controlPoints[i] > 1 and com_controlAgentNearP[i] == 0:
                    #print 'no agents where near %i this one last round, set it back to 1' % i
                    com_controlPoints[i] = 1
                    #if com_controlPScore[i] < 20:
                    #    com_controlPScore[i] = 20

                #if com_controlPoints[i] <= 0: # move weight each round to enemy/neutral points
                #    com_controlPScore[i] += 3
                #elif com_controlAgentNearP[i] > 0:
                #    com_controlPScore[i] -= 1

                if com_controlPScore[i] < 10:
                    com_controlPScore[i] = 10
                elif com_controlPScore[i] > 400:
                    com_controlPScore[i] = 400
                elif com_controlPScore[i] > 250 and com_controlPoints[i] > 0:
                    com_controlPScore[i] = 250

            com_controlAgentNearP = [0, 0, 0]
            print com_controlPScore

        loc = observation['location']

        if observation['respawn']:
            com_agents[observation['id'] - 1] = [0, 0, 0, 0]
            pass # TODO: add prev spot to camper spot?
            #print '%i omfg we got killed at %s' % (round, str(self.prevP.coord))

        preNav(loc,self.team,observation)

        (sh,a,d, enemyID) = shoot(loc,observation)
        #TODO: AMMO COMUNICATION WITH UPDATES

        if sh == 0:
            prevGoal = self.goalID
            if prevGoal != -1: # we had a controlpoint goal last time
                com_controlPScore[prevGoal] = com_controlPScore[prevGoal] + self.lastHeuristicDecrease

            (self.goalID, self.goalScore, heurCPDist) = bestCPGoal(loc,prevGoal,observation) # id and score
            (amScore, bestAmmo) = ammo(loc,observation)
            curGoal = ''

            #Compare prio's ammo/controlpoints
            if amScore > self.goalScore:
                prevGoal = -1
                curGoal = bestAmmo
                com_ammo_nav[str(bestAmmo)] = observation['id']
            else:
                #print 'score: %i %i target: %s' % (self.goalScore, com_controlPScore[self.goalID], level.controls[self.goalID])
                curGoal = level.controls[self.goalID]
                com_agents[observation['id'] - 1][1] = self.goalID

                camperC = 0
                doubleCS = -1
                for i in com_agents:
                    if i[2] == 1 and i[1] == self.goalID: # if controlpoint is the same and it's camping
                        camperC += i[3]
                        if com_agents[observation['id'] - 1][3] == i[3]:
                            doubleCS += 1

                if com_agents[observation['id'] - 1][3] == 0 or doubleCS > 0: # we don't have a campspot side yet (or double somehow..)
                    spot = 0
                    sI = 1
                    if camperC > 0:
                        while 1 << spot & camperC != 0: # find the first free 'spot'/position around CP to defend
                            spot += 1
                        sI = 1 << spot
                        #print '%i %i %i C: %i spot is: %i %i ' % (round, self.goalID, observation['id'] - 1, camperC, spot, sI)
                    com_agents[observation['id'] - 1][3] = sI
                if com_agents[observation['id'] - 1][3] > 1:
                    curGoal = getCampLoc(self.goalID, self.team, com_agents[observation['id'] - 1][3])
                    #print '%i %i %i more camping.. take %i def spot.. %s' % (round, self.goalID, observation['id'] - 1, com_agents[observation['id'] - 1][3] >> 1, str(curGoal))

            self.lastHeuristicDecrease = max(100 - (heurCPDist + 1) * heurCPDist, 10)

            if self.goalID != -1:
                if com_controlPoints[self.goalID] <= 0 and self.lastHeuristicDecrease > 40:
                    self.lastHeuristicDecrease = 40
                com_controlPScore[self.goalID] = com_controlPScore[self.goalID] - self.lastHeuristicDecrease
                #    com_controlPScore[self.goalID] -= 2 # TODO: fix... improve heuri decrease to prevent teammates to go there too

            #print "%i %s bCP: %i %i bA: %s %i hD: %i" % (observation['id'], str(com_controlPScore), self.goalID, self.goalScore, bestAmmo, amScore, self.lastHeuristicDecrease)
            path = []
            node = self.theta_star(loc, curGoal)
            if node:
                while(node.parent != node):
                    path.append(node.loc())
                    node = node.parent
            elif round > 1 and dist(loc, curGoal) > 10:
                pprint(observation)
                print 'omfg WUT no path!!'
                time.sleep(1.5)
                showlevel(loc, curGoal)
            if path == []:
                #print '%i %i %i empty path; looks like we will go camp?' %  (round, self.goalID, observation['id'] - 1)
                com_agents[observation['id'] - 1][2] = 1
                a = getPrefDirection(curGoal, self.team)
                d = 0
                if self.goalID != -1:
                    com_controlPScore[self.goalID] -= 2
            else:
                d = dist(loc,path[-1])
                a = angle(loc,path[-1])

            a -= observation['direction']
            if abs(a) > math.pi:
                if a >= 0:
                    a -= 2*math.pi
                else:
                    a += 2*math.pi
            if abs(a) > math.pi / 3: # TODO: check when we should move to get somewhere faster
                d = 0
        elif sh == 1: # shooting..
            com_agents[observation['id'] - 1][0] = observation['ammo'] - 1 # more accurate update of ammo
            enCPid = com_enemy_vis[enemyID-1][0]
            if -1 != enCPid:
                com_controlPScore[enCPid] = com_controlPScore[enCPid] - WEIGHT_ENEMY_SBonus
            com_enemy_vis[enemyID-1] = [-1,(-1,-1), 0,round]
        else: # gonna shoot in 2-turns
            sh = 0

        action = {
            'turn': a,
            'speed': d,
            'shoot': sh
        }
        return action


    def theta_star(self, start, goal, verbose = False):
        global level
        self.expQueue = []
        closed = []
        s_start = level.tree.addSplit(start[0], start[1], True)
        s_start.g = 0
        s_start.parent = s_start
        s_goal = level.tree.addSplit(goal[0], goal[1], True)

        s_start.val = cost(s_start, s_goal)
        self.expQueue.append(s_start)
        while len(self.expQueue):
            s = self.expQueue.pop(0)
            if s == s_goal:
                return s
            closed.append(s)
            for k in s.neigh:
                if not s.neigh[k] is None: #TEMP fix attempt (if + indent 9 lines) (nout)
                    if verbose == True:
                        print s.neigh[k].loc()
                    s_1 = s.neigh[k]
                    if not line_of_sight(s.loc(), s_1.loc()):
                        continue
                    if not (s_1 in closed):
                        if not (s_1 in self.expQueue):
                            s_1.g = 100000
                            s_1.parent = None
                        self.update_point(s, s_1, s_goal)
        return None

    def update_point(self, s, s_1, s_goal):
        g_old = s_1.g
        compute_cost(s, s_1)
        if s_1.g < g_old:
            if not s_1 in self.expQueue:
                self.expQueue.append(s_1)
            s_1.val = s_1.g + cost(s_1, s_goal)
            self.expQueue.sort()

#Theta star
def compute_cost(s, s_1):
    if line_of_sight(s.parent.loc(), s_1.loc()):
        if s.parent.g + cost(s.parent, s_1) < s_1.g:
            s_1.parent = s.parent
            s_1.g = s.parent.g + cost(s.parent, s_1)
    else:
        if s.g + cost(s, s_1) < s_1.g:
            s_1.parent = s
            s_1.g = s.g + cost(s, s_1)

def cost(s, s_1):
    return sqrt((s_1.x - s.x)*(s_1.x - s.x)+(s_1.y - s.y)*(s_1.y - s.y))

def line_of_sight(s, s_1):
    for wall in level.walls:
        w = wall[0]
        w_1 = wall[1]
        if ((w_1[1]-s[1])*(w[0]-s[0]) > (w[1]-s[1])*(w_1[0]-s[0])) != ((w_1[1]-s_1[1])*(w[0]-s_1[0]) > (w[1]-s_1[1])*(w_1[0]-s_1[0])) \
            and ((w[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w[0]-s[0])) != ((w_1[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w_1[0]-s[0])):
            return False
    for wall in tempWalls:
        w = wall[0]
        w_1 = wall[1]
        if ((w_1[1]-s[1])*(w[0]-s[0]) > (w[1]-s[1])*(w_1[0]-s[0])) != ((w_1[1]-s_1[1])*(w[0]-s_1[0]) > (w[1]-s_1[1])*(w_1[0]-s_1[0])) \
            and ((w[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w[0]-s[0])) != ((w_1[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w_1[0]-s[0])):
            return False

    return True

#Navigation
def preNav(loc,team,observation):
    # some pre Navigation steps..
    # go over all visible agents to see if any of them occupy enemy controlpoint
    # only a controlpoint which we can see needs to be checked (current map it's max 1 same time)
    global com_controlPoints, com_locationsVisited, com_controlAgentNearP, com_agents, com_enemy_vis, com_controlPScore
    id = observation['id']
    cpId = -1
    gP = '' # the goalPoint we can see (if there is one)
    if com_agents[id-1][2] == 1: # we were camping; reset
        com_agents[id-1][2] = 0

    #exit()
    for i in range(3): # for all controlpoints
        #com_controlPoints
        conP = level.controls[i]
        #gPt = Point(controlP[0], controlP[1],0,False)
        d = dist(loc, conP)
        if d <= MAX_SIGHT:
            cpId = i
            gP = conP

            #if com_controlPoints[cpId] == 0 or com_controlPoints[cpId] == 1:
                #self.path.appendleft(gP) # neutral point close by.. let's reset to see if we should get/defend this
                #resetting_path = cpId

            if d <= MAX_SIGHT - CONTROLPOINT_SIZE:   # so that we can see the full point
                com_controlAgentNearP[cpId] = 1
                if com_controlPoints[cpId] == -2: # last state it was theirs + occupied
                    com_controlPoints[cpId] = -1 # set it to just 'theirs' and see if it's occupied next

                if com_controlPoints[cpId] > 0 and line_of_sight(loc, conP): #standing near it AND it's ours
                    if com_controlPoints[cpId] == 1: # previously wasn't someone on/near
                        com_controlPoints[cpId] = 2
                    #print '%i standing and owning %s' % (id, str(conP))
                    if com_agents[id-1][2] != 2: # aren't running to make spot so should go camping
                        #TODO: add check for teammate camping here?
                        #print '%i NEW/Reached goal %i %i' % (id, controlP[0], controlP[1])
                        com_agents[id-1][2] = 1 # we're now camping?
                        #self.goalInt = cpId
                        #self.camping = 1

                if d <= CONTROLPOINT_SIZE: # we are standing on the controlpoint
                    if com_controlPoints[cpId] > 0: #standing on it AND it's ours
                        if com_controlPoints[cpId] == 1: # previously wasn't someone on
                            com_controlPoints[cpId] = 2
                        #print '%i standing and owning %s' % (id, str(conP))
                        if observation['ammo'] > 0:
                            com_controlPoints[cpId] = 3
                    #else: # we are standing on CP but it's not ours.. let's move!
    if com_agents[id-1][2] != 1:
        #print 'not camping! (anymore?)'
        com_agents[observation['id'] - 1][3] = 0
    for agent in observation['agents']:
        al = agent['location']
        level.tree.addSplit(al[0]+5,al[1]+10,True)
        level.tree.addSplit(al[0]+10,al[1]+5,True)
        level.tree.addSplit(al[0]-5,al[1]+10,True)
        level.tree.addSplit(al[0]-10,al[1]+5,True)
        level.tree.addSplit(al[0]+5,al[1]-10,True)
        level.tree.addSplit(al[0]+10,al[1]-5,True)
        level.tree.addSplit(al[0]-5,al[1]-10,True)
        level.tree.addSplit(al[0]-10,al[1]-5,True)
        tempWalls.append(((al[0]+4.9,al[1]+9.9),(al[0]+9.9,al[1]+4.9)))
        tempWalls.append(((al[0]+9.9,al[1]+4.9),(al[0]+9.9,al[1]-4.9)))
        tempWalls.append(((al[0]+9.9,al[1]-4.9),(al[0]+4.9,al[1]-9.9)))
        tempWalls.append(((al[0]+4.9,al[1]-9.9),(al[0]-4.9,al[1]-9.9)))
        tempWalls.append(((al[0]-4.9,al[1]-9.9),(al[0]-9.9,al[1]-4.9)))
        tempWalls.append(((al[0]-9.9,al[1]-4.9),(al[0]-9.9,al[1]+4.9)))
        tempWalls.append(((al[0]-9.9,al[1]+4.9),(al[0]-4.9,al[1]+9.9)))
        tempWalls.append(((al[0]-4.9,al[1]+9.9),(al[0]+4.9,al[1]+9.9)))


        if agent['team'] != team: # for now we only care about the opponents
            #if str(agent['location']) in com_enemy_nav: %i enemy %s getting shot!
            enID = agent['id'] - 1
            ep = (agent['location'][0],agent['location'][1])
            if cpId >= 0:
                if com_enemy_vis[enID][0] != cpId:
                    com_enemy_vis[enID][0] = cpId
                    com_controlPScore[cpId] = com_controlPScore[cpId] + WEIGHT_ENEMY_SBonus # extra weight to this point cuz there is an enemy to beat..
                    #print 'EXTRA WEIGHT %i' % cpId # TODO: remove weight if shooting this enemy?

                if com_controlPoints[cpId] == -1: # check if an enemy is on this spot
                    d = dist(gP, ep)
                    if d <= CONTROLPOINT_SIZE:
                        com_controlPoints[cpId] = -2 # taken + defended
                #TODO: check in how many turns we can kill this enemy and put the enemy in a prio list to shoot
            if com_enemy_vis[enID][3] != round:
                com_enemy_vis[enID] = [cpId,ep, com_enemy_vis[enID][1] == ep,round]

    #for locations in com_locationsVisited:
        #[x, y, score] = locations
        #ep = Point(x, y,0,False)
        #if cp.getDist(ep) < 20: # TODO, get a more logical check here?
            #locations[2] = round # currently checks if it's next to this point but point
            #print 'we are at %i %i (for ammo checking) new score: %i' % (x, y, round)
            # where it should go to and point where it checks should perhaps be different
    return


def shoot(loc,observation):
    global com_enemy_shot
    if observation['ammo'] == 0:
        return (0,0,0, -1)
    #TODO: CHOOSE BEST TARGET; the one requiring least turns? or double-shot target?
    sa = 0
    sd = 0
    sh = 0
    sID = 0
    for agent in observation['agents']:
        if agent['team'] != observation['team']:
            if str(agent['location']) not in com_enemy_shot and line_of_sight(loc,agent['location']):
                a = angle(loc,agent['location'])
                d = dist(loc,agent['location'])

                a -= observation['direction']
                if abs(a) > math.pi:
                    if a >= 0:
                        a -= 2*math.pi
                    else:
                        a += 2*math.pi

                #TODO: R U CAMPING? IS THIS GOOD SHOOTING BEHAVIOUR?
                sh = 2
                if abs(a) <= math.pi / 3 and (int(math.ceil(d / MAX_SHOT_DISTANCE)) - 1) <= 0:
                    com_enemy_shot[str(agent['location'])] = observation['id']
                    #print 'SHOOTING!!'
                    return (1, a, 0, agent['id']) # no need to check other enemies, perhaps move aswell tho?
                if com_agents[observation['id']-1][2] == 1: # we're camping and on spot
                    sd = 0
                elif (int(math.ceil(d / MAX_SHOT_DISTANCE)) - 1) <= 0:
                    sd = 0
                else:
                    sd = d - (MAX_SHOT_DISTANCE - 1)
                sID = agent['id']
                sa = a
    if sh == 2:
        #print '%i 2-step targetting! %i camping %i x/y: %s ' % (observation['id'], sd, com_agents[observation['id']-1][2], str(observation['location']))
        return (sh, sa, sd, sID) # targetting a 2 step target
    return (0, 0, 0, -1) #aw, nothing to shoot :/

def ammo(loc,observation):
    global com_ammo_nav
    bestAmmoScore = -999
    bestAmmo = ''
    for ammo_av in observation['ammopacks']:
        if str(ammo_av['location']) not in com_ammo_nav:
            #TODO: communicate this ammo point to teammates

            if line_of_sight(loc,ammo_av['location']):
                d = dist(loc,ammo_av['location'])
                a = angle(loc,ammo_av['location'])

                a -= observation['direction']
                if abs(a) > math.pi:
                    if a >= 0:
                        a -= 2*math.pi
                    else:
                        a += 2*math.pi

                movesReq = (int(math.ceil(abs(a) / (math.pi / 3))) - 1) # turns required
                movesReq += int(math.ceil(d / MAX_SPEED)) - 1 # dist steps req
                ammoScore = WEIGHT_AM_BASE - WEIGHT_AM_AMMO * (observation['ammo'] - 1) * observation['ammo']  - WEIGHT_AM_MOVES * movesReq * movesReq # TODO: improve estimate ammoScore

                if bestAmmoScore < ammoScore:
                    bestAmmoScore = ammoScore
                    bestAmmo = ammo_av['location']

    #TODO: GOTO POINT LIKELY TO HAVE AMMO with some score (communicated by teammates or unvisited areas)
    return (bestAmmoScore, bestAmmo)

def bestCPGoal(loc,prevGoalID, observation):
    global level, com_controlPScore, com_controlPoints
    bestGoal = -1
    bestScore = -999
    bestHeurDist = 99
    for i in range(3): # for all controlpoints
        conP = level.controls[i]
        heurD = getHeuristicDist(loc, conP)
        scoreP = com_controlPScore[i] - heurD # global - heuristic number of turns to get at point
        if prevGoalID == i: # we had this as goal, let's try to stick to it
            scoreP += WEIGHT_BESTCP_PREV
        if scoreP > bestScore and (com_controlPoints[i] >= -1 or observation['ammo'] > 0):
            bestScore = scoreP
            bestGoal = i
            bestHeurDist = heurD
    #TODO: TURN THIS IN NICE WEIGHT FUNC GIVEN OBS AND PREV GOAL
    #while dist(loc,level.controls[self.goalID]) < CONTROLPOINT_SIZE:
    #    self.goalID = random.randint(0,2)
    return (bestGoal, bestScore, bestHeurDist)

def dist(a,b):
    x = b[0]-a[0]
    y = b[1]-a[1]
    d = math.sqrt(x**2+y**2)
    return d

def angle(a,b):
    x = b[0]-a[0]
    y = b[1]-a[1]
    a = -math.atan2(-y,x)
    return a


def getHeuristicDist(a, b):
    #Compute the distance between the points + a 'bonus' for different area's
    d = dist(a, b)
    aR = getRegion(a)
    bR = getRegion(b)
    if (aR == 0 and bR == 1): #TODO: add/modify bonus weights?
        d += 500
    elif aR != bR:
        d += 150
    return int(d / MAX_SPEED) # roughly numbers of turns before getting there

#Get the region, pretty hard-coded
def getRegion(p):
    (x,y) = p
    if x > 798 or x < 2 or y > 798 or y < 2:
        return -1 # to prevent going to ammo spawnpoints on walls
    if x >= 600:
        if y < 400:
            return 0 # region A
        return 1 # region B
    if x >= 300:
        return 5 # region F
    if y >= 500:
        return 2 # region C
    if y >= 300:
        return 3 # region D
    return 4 # region E

# Prefered direction when camping
def getPrefDirection(xy, team):
    (x,y) = xy
    if x > 120 and x < 320:
        if y > 480 and y < 680:
            return 0.75*math.pi
        #elif x == 220 and y == 220:
        return -0.75*math.pi
    if team == 'Red':
        return -0.5*math.pi
    else:
        return 0.5*math.pi

def getCampLoc(goalID, team, nr2):
    nr  = nr2 >> 1
    #print 'getCampLoc: %i %i nr2: %i nr: %i' % (x, y, nr2, nr)
    if 1 == goalID: # left-upper point
            if 1 == nr:
                return (280, 160)
            elif 2 == nr:
                return (160, 280)
            return (100, 100)
        #580 == y
    elif 2 == goalID:
        if 1 == nr: # left-down
            return (280, 640)
        elif 2 == nr:
            return (160, 520)
        return (100, 700)
    if team == 'Red': # 'middle' CP
        if 1 == nr:
            return (440, 340)
        elif 2 == nr:
            return (560, 340)
        return (340, 340)
    else:
        if 1 == nr:
            return (440, 460)
        elif 2 == nr:
            return (560, 460)
        return (340, 460)

def showlevel(start, goal):
    global level
    agent = AgentBrain()
    path = agent.theta_star(start, goal, False)


    pygame.init()
    window = pygame.display.set_mode((800, 800))
    level.displayUpdate(level.tree, window)
    pygame.draw.circle(window, (0, 255, 255), start, 5)
    pygame.draw.circle(window, (255, 255, 0), goal, 5)
    print "start:",start
    print "goal:",goal
    if not path is None:
        nextpath = path.parent

        while(path != nextpath):
            pygame.draw.line(window, (102, 194, 33), path.loc(), nextpath.loc())
            path = nextpath
            nextpath = path.parent
        pygame.display.update()

    for w in level.walls:
        pygame.draw.line(window, (255, 0, 0), w[0], w[1])
    for w in tempWalls:
        pygame.draw.line(window, (0, 255, 0), w[0], w[1])
    pygame.display.update()

    while True:
       for event in pygame.event.get():
            if event.type == pygame.QUIT:
                #sys.exit(0)
                return
